//引入PS2X库文件
#include <PS2X_lib.h>

//定义ps2x手柄相关变量
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;

//定义输入输出引脚
int input1 = 4;
int input2 = 5;
int input3 = 6;
int input4 = 7;

void setup() {
  //  初始化引脚
  pinMode(input1, OUTPUT);
  pinMode(input2, OUTPUT);
  pinMode(input3, OUTPUT);
  pinMode(input4, OUTPUT);

  Serial.begin(57600);
  delay(300);
  // config_gamepad(clock, command, attention, data, Pressures?, Rumble?);
  error = ps2x.config_gamepad(13, 11, 10, 12, true, true);
  //判断连接状态
  if (error == 0) {
    Serial.println("发现控制器，配置成功");
    Serial.println("尝试所有的按钮,X会震动控制器,越用力越快");
    Serial.println("按住L1或R1将打印出模拟摇杆值");
  } else if (error == 1)
    Serial.println("没有找到控制器，检查接线");
  else if (error == 2)
    Serial.println("找到控制器，但不接受命令");
  else if (error == 3)
    Serial.println("控制器拒绝进入压力模式，可能不支持");
  //判断连接类型
  type = ps2x.readType();
  switch (type) {
    case 0:
      Serial.println("未知的控制器类型");
      break;
    case 1:
      Serial.println("找到DualShock控制器");
      break;
    case 2:
      Serial.println("找到GuitarHero控制器");
      break;
    case 3:
      Serial.println("找到无线索尼双冲击控制器");
      break;
  }
}

void loop() {
  if (error == 1) {
    //如果没连接上手柄,输出提示,并尝试重连,return结束本次循环
    Serial.println("没找到");
    reConect();
    return;
  }
  //连接上手柄后,对手柄类型进行判断
  if (type == 2) {        //Guitar Hero Controller
    ps2x.read_gamepad();  //读取控制器按键

    if (ps2x.ButtonPressed(GREEN_FRET))
      Serial.println("Green Fret Pressed");
    if (ps2x.ButtonPressed(RED_FRET))
      Serial.println("Red Fret Pressed");
    if (ps2x.ButtonPressed(YELLOW_FRET))
      Serial.println("Yellow Fret Pressed");
    if (ps2x.ButtonPressed(BLUE_FRET))
      Serial.println("Blue Fret Pressed");
    if (ps2x.ButtonPressed(ORANGE_FRET))
      Serial.println("Orange Fret Pressed");

    if (ps2x.ButtonPressed(STAR_POWER))
      Serial.println("Star Power Command");

    if (ps2x.Button(UP_STRUM))  //只要按下按钮，就是true
      Serial.println("Up Strum");
    if (ps2x.Button(DOWN_STRUM))
      Serial.println("DOWN Strum");

    if (ps2x.Button(PSB_START))  //只要按下按钮，就是true
      Serial.println("Start is being held");
    if (ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");

    if (ps2x.Button(ORANGE_FRET)) {  // print stick value IF TRUE
      Serial.print("Wammy Bar Position:");
      Serial.println(ps2x.Analog(WHAMMY_BAR), DEC);
    }
  } else {                              //DualShock Controller
    ps2x.read_gamepad(false, vibrate);  //read controller and set large motor to spin at 'vibrate' speed

    if (ps2x.Button(PSB_START))  //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if (ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");
    // ======这里填入 上下左右键 对应的行动======
    if (ps2x.Button(PSB_PAD_UP)) {  //will be TRUE as long as button is pressed
      Serial.print("Up held this hard: ");

      RUN();

      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if (ps2x.Button(PSB_PAD_RIGHT)) {
      Serial.print("Right held this hard: ");
      LEFT();
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if (ps2x.Button(PSB_PAD_LEFT)) {
      Serial.print("LEFT held this hard: ");
      RIGHT();
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if (ps2x.Button(PSB_PAD_DOWN)) {
      Serial.print("DOWN held this hard: ");
      BACK();
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }

    vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
    if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
      if (ps2x.Button(PSB_L3))
        Serial.println("L3 pressed");
      if (ps2x.Button(PSB_R3))
        Serial.println("R3 pressed");
      if (ps2x.Button(PSB_L2))
        Serial.println("L2 pressed");
      if (ps2x.Button(PSB_R2))
        Serial.println("R2 pressed");
      if (ps2x.Button(PSB_TRIANGLE))
        Serial.println("Triangle pressed");
      STOP();
    }

    if (ps2x.ButtonPressed(PSB_CIRCLE))  //will be TRUE if button was JUST pressed
      Serial.println("Circle just pressed");
    if (ps2x.NewButtonState(PSB_CROSS))  //will be TRUE if button was JUST pressed OR released
      Serial.println("X just changed");
    if (ps2x.ButtonReleased(PSB_SQUARE))  //will be TRUE if button was JUST released
      Serial.println("Square just released");

    if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) {  //print stick values if either is TRUE
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC);  //Left stick, Y axis. Other options: LX, RY, RX
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC);
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC);
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC);
    }
  }
  delay(50);
}

//测试用例
// void loop(){
//   // RUN();
//   // delay(2000);
//   // STOP();
//   // delay(2000);
//   STOP();
// }

//停止函数
void STOP() {
  digitalWrite(input1, LOW);
  digitalWrite(input2, LOW);
  digitalWrite(input3, LOW);
  digitalWrite(input4, LOW);
}

//前进函数
void RUN() {
  digitalWrite(input1, HIGH);
  digitalWrite(input2, LOW);
  digitalWrite(input3, HIGH);
  digitalWrite(input4, LOW);
}

//左转函数
void LEFT() {
  digitalWrite(input1, LOW);
  digitalWrite(input2, HIGH);
  digitalWrite(input3, HIGH);
  digitalWrite(input4, LOW);
}

//右转函数
void RIGHT() {
  digitalWrite(input1, HIGH);
  digitalWrite(input2, LOW);
  digitalWrite(input3, LOW);
  digitalWrite(input4, HIGH);
}

//后退函数
void BACK() {
  digitalWrite(input1, LOW);
  digitalWrite(input2, HIGH);
  digitalWrite(input3, LOW);
  digitalWrite(input4, HIGH);
}

// 重连
void reConect(){
  // config_gamepad(clock, command, attention, data, Pressures?, Rumble?);
  error = ps2x.config_gamepad(13, 11, 10, 12, true, true);

  if (error == 0) {
    Serial.println("发现控制器，配置成功");
    Serial.println("尝试所有的按钮,X会震动控制器,越用力越快");
    Serial.println("按住L1或R1将打印出模拟摇杆值");
  } else if (error == 1)
    Serial.println("没有找到控制器，检查接线");
  else if (error == 2)
    Serial.println("找到控制器，但不接受命令");
  else if (error == 3)
    Serial.println("控制器拒绝进入压力模式，可能不支持");

  type = ps2x.readType();
  switch (type) {
    case 0:
      Serial.println("未知的控制器类型");
      break;
    case 1:
      Serial.println("找到DualShock控制器");
      break;
    case 2:
      Serial.println("找到GuitarHero控制器");
      break;
    case 3:
      Serial.println("找到无线索尼双冲击控制器");
      break;
  }
}